#include "DeviceMessageParserV2.h"
#include <algorithm>
#include <cstring>

namespace DeviceCommons {

    // ==================== Helper Functions Implementation ====================
    
    std::vector<uint8_t> hexToBytes_impl(const std::string& hex) {
        if (hex.length() % 2 != 0) {
            throw std::invalid_argument("Invalid hex string length");
        }
        
        std::vector<uint8_t> bytes;
        for (size_t i = 0; i < hex.length(); i += 2) {
            std::string byteString = hex.substr(i, 2);
            uint8_t byte = static_cast<uint8_t>(strtol(byteString.c_str(), nullptr, 16));
            bytes.push_back(byte);
        }
        return bytes;
    }

    uint16_t crc16_parser_impl(const std::vector<uint8_t>& data) {
        uint16_t crc = 0x0000;
        for (uint8_t b : data) {
            crc ^= uint16_t(b) << 8;
            for (int i = 0; i < 8; ++i)
                crc = (crc & 0x8000) ? (crc << 1) ^ 0x8005 : crc << 1;
            crc &= 0xFFFF;
        }
        return crc;
    }

    // ==================== DeviceMessageParserV2 Implementation ====================
    
    DeviceMessageParserV2::ParsedMessage DeviceMessageParserV2::parseFromHex(const std::string& hexString) {
        std::vector<uint8_t> bytes = hexToBytes_impl(hexString);
        return parseFromBytes(bytes);
    }

    DeviceMessageParserV2::ParsedMessage DeviceMessageParserV2::parseFromBytes(const std::vector<uint8_t>& data) {
        if (data.size() < 4) {
            throw std::invalid_argument("Message too short - missing header");
        }

        ParsedMessage result;
        size_t offset = 0;

        // 1. Parse protocol header
        result.protocolHeader[0] = data[offset++];
        result.protocolHeader[1] = data[offset++];
        
        if (result.protocolHeader[0] != 0xC0 || result.protocolHeader[1] != 0xBF) {
            throw std::invalid_argument("Invalid protocol header");
        }

        // 2. Parse version (V2 only)
        result.version = data[offset++];
        
        if (result.version != PROTOCOL_VERSION_V2) {
            throw std::invalid_argument("Unsupported protocol version - only V2 is supported");
        }
        
        // 3. Parse Mark byte
        result.mark = data[offset++];
        parseMarkByte(result.mark, result);

        // 4. Parse V2 protocol data
        parseV2Protocol(data, offset, result);

        return result;
    }

    void DeviceMessageParserV2::parseMarkByte(uint8_t mark, ParsedMessage& result) {
        result.timeStampFormat = static_cast<TimeStampFormat>(mark & 0x01);
        result.headerValueType = static_cast<HeaderValueType>((mark >> 1) & 0x01);
        result.reserve1 = static_cast<Reserve1>((mark >> 2) & 0x01);
        result.reserve2 = static_cast<Reserve2>((mark >> 3) & 0x01);
        result.crcType = static_cast<CRCType>((mark >> 4) & 0x0F);
    }

    void DeviceMessageParserV2::parseV2Protocol(const std::vector<uint8_t>& data, size_t offset, ParsedMessage& result) {
        if (offset >= data.size()) {
            throw std::invalid_argument("Unexpected end of data");
        }

        // 1. Parse device count
        uint8_t deviceCount = data[offset++];
        
        if (deviceCount == 0) {
            throw std::invalid_argument("No devices in message");
        }

        // 2. Parse all devices (first is main device, rest are child devices)
        std::vector<DeviceInfo> allDevices;
        for (uint8_t i = 0; i < deviceCount; i++) {
            DeviceInfo device = parseDeviceInfo(data, offset);
            allDevices.push_back(device);
        }

        // 3. Separate main device and child devices
        result.mainDevice = allDevices[0];  // First device is main device
        
        // Rest are child devices
        for (size_t i = 1; i < allDevices.size(); i++) {
            result.childDevices.push_back(allDevices[i]);
        }

        // 4. Verify CRC (if enabled)
        verifyCRC(data, offset, result);
    }

    DeviceInfo DeviceMessageParserV2::parseDeviceInfo(const std::vector<uint8_t>& data, size_t& offset) {
        if (offset >= data.size()) {
            throw std::invalid_argument("Unexpected end of data while parsing device");
        }

        DeviceInfo device;
        
        // 1. Parse device ID length and content
        uint8_t didLength = data[offset++];
        if (offset + didLength > data.size()) {
            throw std::invalid_argument("Invalid device ID length");
        }
        
        device.did = std::string(data.begin() + offset, data.begin() + offset + didLength);
        offset += didLength;

        // 2. Parse device type
        if (offset >= data.size()) {
            throw std::invalid_argument("Missing device type");
        }
        device.deviceType = data[offset++];

        // 3. Parse reading count
        if (offset >= data.size()) {
            throw std::invalid_argument("Missing reading count");
        }
        uint8_t readingCount = data[offset++];

        // 4. Parse all readings
        for (uint8_t i = 0; i < readingCount; i++) {
            Reading reading = parseReading(data, offset);
            device.readings.push_back(reading);
        }

        return device;
    }

    Reading DeviceMessageParserV2::parseReading(const std::vector<uint8_t>& data, size_t& offset) {
        if (offset + 2 > data.size()) {
            throw std::invalid_argument("Insufficient data for reading");
        }

        Reading reading;
        
        // 1. Parse time offset (big-endian format)
        reading.timeOffset = static_cast<int16_t>((data[offset] << 8) | data[offset + 1]);
        offset += 2;

        // 2. Parse state count
        if (offset >= data.size()) {
            throw std::invalid_argument("Missing state count");
        }
        uint8_t stateCount = data[offset++];

        // 3. Parse all states
        for (uint8_t i = 0; i < stateCount; i++) {
            State state = parseState(data, offset);
            reading.states.push_back(state);
        }

        return reading;
    }

    State DeviceMessageParserV2::parseState(const std::vector<uint8_t>& data, size_t& offset) {
        if (offset + 2 > data.size()) {
            throw std::invalid_argument("Insufficient data for state");
        }

        State state;
        
        // 1. Parse state ID and type
        state.sid = data[offset++];
        state.type = static_cast<StateValueType>(data[offset++]);

        // 2. Parse value based on type
        switch (state.type) {
            case StateValueType::String: {
                if (offset >= data.size()) {
                    throw std::invalid_argument("Missing string length");
                }
                uint8_t strLength = data[offset++];
                if (offset + strLength > data.size()) {
                    throw std::invalid_argument("Invalid string length");
                }
                state.value.assign(data.begin() + offset, data.begin() + offset + strLength);
                offset += strLength;
                break;
            }
            case StateValueType::Float32:
            case StateValueType::Int32:
                if (offset + 4 > data.size()) {
                    throw std::invalid_argument("Insufficient data for 32-bit value");
                }
                state.value.assign(data.begin() + offset, data.begin() + offset + 4);
                offset += 4;
                break;
            case StateValueType::Bool:
                if (offset >= data.size()) {
                    throw std::invalid_argument("Missing boolean value");
                }
                state.value.push_back(data[offset++]);
                break;
            case StateValueType::UInt16:
            case StateValueType::Int16:
                if (offset + 2 > data.size()) {
                    throw std::invalid_argument("Insufficient data for 16-bit value");
                }
                state.value.assign(data.begin() + offset, data.begin() + offset + 2);
                offset += 2;
                break;
            case StateValueType::Double:
            case StateValueType::Timestamp:
                if (offset + 8 > data.size()) {
                    throw std::invalid_argument("Insufficient data for 64-bit value");
                }
                state.value.assign(data.begin() + offset, data.begin() + offset + 8);
                offset += 8;
                break;
            case StateValueType::Binary: {
                if (offset + 2 > data.size()) {
                    throw std::invalid_argument("Missing binary length");
                }
                uint16_t binLength = (data[offset] << 8) | data[offset + 1];
                offset += 2;
                if (offset + binLength > data.size()) {
                    throw std::invalid_argument("Invalid binary length");
                }
                state.value.assign(data.begin() + offset, data.begin() + offset + binLength);
                offset += binLength;
                break;
            }
            default:
                throw std::invalid_argument("Unknown state value type");
        }

        return state;
    }

    void DeviceMessageParserV2::verifyCRC(const std::vector<uint8_t>& data, size_t offset, ParsedMessage& result) {
        result.crcValid = true;  // Default to true if no CRC check needed

        if (result.crcType == CRCType::CRC16) {
            if (offset + 2 > data.size()) {
                result.crcValid = false;
                result.calculatedCRC = 0;
                result.messageCRC = 0;
                return;
            }

            // Extract CRC from message (little-endian)
            result.messageCRC = data[offset] | (data[offset + 1] << 8);

            // Calculate CRC for all data except the CRC bytes
            std::vector<uint8_t> dataForCRC(data.begin(), data.begin() + offset);
            result.calculatedCRC = crc16_parser_impl(dataForCRC);

            result.crcValid = (result.calculatedCRC == result.messageCRC);
        }
    }

    // ==================== Helper Functions Implementation ====================
    
    void printParsedMessage(const DeviceMessageParserV2::ParsedMessage& msg) {
        std::cout << "=== Parsed Message Information ===" << std::endl;
        std::cout << "Protocol Header: 0x" << std::hex << (int)msg.protocolHeader[0] 
                  << " 0x" << (int)msg.protocolHeader[1] << std::dec << std::endl;
        std::cout << "Version: " << (int)msg.version << std::endl;
        std::cout << "Mark Byte: 0x" << std::hex << (int)msg.mark << std::dec << std::endl;
        std::cout << "CRC Valid: " << (msg.crcValid ? "Yes" : "No") << std::endl;
        
        if (msg.crcType == CRCType::CRC16) {
            std::cout << "CRC16 - Calculated: 0x" << std::hex << msg.calculatedCRC 
                      << ", Message: 0x" << msg.messageCRC << std::dec << std::endl;
        }
        
        std::cout << "Main Device: " << msg.mainDevice.did << " (Type: " 
                  << (int)msg.mainDevice.deviceType << ")" << std::endl;
        std::cout << "Child Devices: " << msg.childDevices.size() << std::endl;
        
        for (size_t i = 0; i < msg.childDevices.size(); ++i) {
            std::cout << "  [" << i << "] " << msg.childDevices[i].did 
                      << " (Type: " << (int)msg.childDevices[i].deviceType << ")" << std::endl;
        }
        
        std::cout << "================================" << std::endl;
    }

} // namespace DeviceCommons